/*
gyro.c
提供从陀螺仪获取角度信息。
designed by linxiantao

--------------------------------
2020-05-26 10:44:00
create file
--------------------------------
*/

#include <aos/hal/uart.h>
#include "gyro.h"
#include "wt901c485.h"
#include "joint.h"

#define UART1_PORT_NUM  5
#define RESET_IO_NUM 4

#define UART_BUF_SIZE   ((GYRO_FRAME_LEN)*2)
#define UART_RX_TIMEOUT 50
#define GYRO_FRAME_LEN 13

static u8 g_initflag = 0;
static u8 g_if_print = 0;
float g_angle_zero_offset = 0;

/* define dev */
static uart_dev_t gyro_uart;
static float gyro_angle, gyro_angle_rate;
static int16_t gyro_status;

static void angle_change(float *angle)
{
	if(*angle <= 0 && *angle >= -180){
		if(angle != 0){
			*angle = 360 + *angle;
		}		
	}
}

void xp_gyro_reset()
{
	println("--------xp_gyro_reset--------");
	xp_agv_io_set(RESET_IO_NUM, 0);
	aos_msleep(500);
	xp_agv_io_set(RESET_IO_NUM, 1);
}

int xp_gyro_safe_reset()
{
	int32_t sta = 0;
	uint64_t s_time = 0, c_time = 0;

	xp_gyro_reset();
	aos_msleep(4000);
	
	s_time = aos_now_ms();
	do{
		c_time = aos_now_ms();
		if(c_time - s_time > 8000){
			println("--------gyro_reset wait timeout!!!--------");
			return -1;
		}
		aos_msleep(1000);
		sta = get_gyro_status();
		if(sta != 0){
			println("--------gyro_sta != 0, wait...--------");
		}
	}while(sta != 0);
	
	println("--------gyro_reset success!--------");

	return 0;
}

int xp_gyro_soft_reset(int8_t side)
{
	uint8_t i = 0;
	float angle_sum = 0;

	for(i = 0; i < 5; i++){
		angle_sum += gyro_angle;
		aos_msleep(100);
	}
	
	g_angle_zero_offset = angle_sum / 5 - side * 90 % 360;
	println("xp_gyro_soft_reset: side %d zero_offset is %.1f", side, g_angle_zero_offset);
}

float get_gyro_angle()
{
	return (((int32_t)(gyro_angle * 1000) + 360000 - (int32_t)(g_angle_zero_offset * 1000)) % 360000) / 1000.0f;
}

float get_gyro_angle_rate()
{
	return gyro_angle_rate;
}

int16_t get_gyro_status()
{
	return gyro_status;
}

static int32_t find_gyro_frame(char *buf)
{
	int16_t head;
	int16_t end;
	int start_index = 0;

	while(start_index <= GYRO_FRAME_LEN)
	{
		head = (buf[start_index+1]<<8) | buf[start_index+0];
	 	end = (buf[start_index+11]<<8) | buf[start_index+12];
		
		if(head == (int16_t)0xFFFF && end == (int16_t)0x0D0A)
		{
			return start_index;
		}
		start_index++;
	}
}


static int32_t parsing_gyro_packet(char *buf)
{	
	int16_t head = (buf[1]<<8) | buf[0];
	int16_t end = (buf[11]<<8) | buf[12];
	
	if(head != (int16_t)0xFFFF)
	{
		return -1;
	}
	
	if(end != (int16_t)0x0D0A)
	{
		return -1;
	}
	
	int16_t rate = (int16_t)(buf[3]<<8) | buf[2];
	
	int16_t angle = (int16_t)(buf[5]<<8) | buf[4];

	int16_t status = (int16_t)(buf[9]<<8) | buf[8];

	int16_t checkSum = (buf[7]<<8) | buf[6];
	if(checkSum != head + rate + angle)
	{
		return -1;
	}
	
	gyro_angle = (float)angle/100.0f;
	angle_change(&gyro_angle);
	gyro_angle = 360 - gyro_angle;//转换成逆时针角度变大
	gyro_angle_rate = (float)rate/100.0f;
	gyro_status = status;
}

static void gyro_data_recv_detect(void *param)
{
	int32_t ret = 0;
	int rx_size = 0;
	/* data buffer */
	char uart_data_buf[UART_BUF_SIZE];
	int32_t start_index;

    while(1) {
			ret = hal_uart_recv_II(&gyro_uart, uart_data_buf, UART_BUF_SIZE,
											 &rx_size, UART_RX_TIMEOUT);
			if ((ret == 0) && (rx_size == UART_BUF_SIZE)) {
				start_index = find_gyro_frame(uart_data_buf);
				parsing_gyro_packet(&uart_data_buf[start_index]);
			}
            if(g_if_print)
            {
              printf("%f,%f \n", gyro_angle, gyro_angle_rate);
            }
      /*
        这里延时最好10ms 因为模块是100hz输出，如果这里延时变多，会导致
        得到的角度滞后
      */
      
      aos_msleep(10);
    }
}


int32_t xp_gyro_init()
{	
	int32_t ret = 0;
	if(g_initflag == 1)
	{
        return 0;
    }
	/* uart port set */
	gyro_uart.port = UART1_PORT_NUM;

	/* uart attr config */
	gyro_uart.config.baud_rate	  = 115200;
	gyro_uart.config.data_width   = DATA_WIDTH_8BIT;
	gyro_uart.config.parity 	  = NO_PARITY;
	gyro_uart.config.stop_bits	  = STOP_BITS_1;
	gyro_uart.config.flow_control = FLOW_CONTROL_DISABLED;
	gyro_uart.config.mode		  = MODE_TX_RX;

	/* init uart1 with the given settings */
	ret = hal_uart_init(&gyro_uart);
	if (ret != 0) {
		printf("127 uart1 init error !\n");
		return -1;
	}
	if (0 != aos_task_new("gyro", gyro_data_recv_detect, NULL, 2048)) {
        printf("create gyro thread fail");
		return -1;
    }

	/* wait gyro reset complete */
	robot_stop();
	aos_msleep(3000);
	xp_gyro_safe_reset();

    g_initflag = 1;
	
	return 0;
}

/**
* @description: xp_washwork_debug(char *type,char *fun,char *param)
* @param param:type: 
*              fun function 
*              param  
* @return: 
*/
int xp_gyro_debug(char *type,char *fun,char *param)
{
    if(strcmp(type,"gyro")!=0)return 0;
    if(strcmp(fun,"print")==0)
    {
       g_if_print = 1;
    }
    else if(strcmp(fun,"cprint")==0)
    {
        g_if_print =0;
    }
    else
    {
        return 0;
    }
    return 1;

}



